
% 单视图标定 另一个图像demo.jpg 测试文件
clc
clear all
% 单视图标定
% 平面上的平行线 的交点 
% x0z面
points1_line1_2dxz = [404.21428571428567, 209.82142857142856];
points2_line1_2dxz = [522 199];
points1_line2_2dxz = [457.7857142857142, 666.9642857142857];
points2_line2_2dxz = [560.4642857142857, 641.0714285714286];
% x0y面
points1_line1_2dxy = [633.6785714285713, 695.5357142857142];
points2_line1_2dxy = [894.392857142857, 912.4999999999999];
points1_line2_2dxy = [678.3214285714284, 682.1428571428571];
points2_line2_2dxy = [945.2857142857142, 887.4999999999999];
% y0z面
points1_line1_2dyz = [956.8928571428571, 319.6428571428571];
points2_line1_2dyz = [942.6071428571427, 539.2857142857142];
points1_line2_2dyz = [1141.7142857142856, 362.49999999999994];
points2_line2_2dyz = [1113.142857142857, 614.2857142857142];

% 计算直线向量与影消点
% 采取svd方式
% [~,~, vm] = svd([[points1_line1_2dxz,1]; [points2_line1_2dxz,1]],0);
% line1_vec_2dxz = vm(:,end); 
% [~,~, vm] = svd([[points1_line2_2dxz,1];[points2_line2_2dxz,1]],0);
% line2_vec_2dxz = vm(:,end); 
% [~,~, vm] = svd([line1_vec_2dxz'; line2_vec_2dxz']);
% ShadowEliminationPoint_xz = vm(:,end);
% 
% [~,~, vm] = svd([[points1_line1_2dxy,1]; [points2_line1_2dxy,1]],0);
% line1_vec_2dxy = vm(:,end); 
% [~,~, vm] = svd([[points1_line2_2dxy,1];[points2_line2_2dxy,1]],0); 
% line2_vec_2dxy = vm(:,end); [~,~, vm] =svd([line1_vec_2dxy'; line2_vec_2dxy']); 
% ShadowEliminationPoint_xy =vm(:,end);
% 
% [~,~, vm] = svd([[points1_line1_2dyz,1]; [points2_line1_2dyz,1]],0);
% line1_vec_2dyz = vm(:,end);
% [~,~, vm] = svd([[points1_line2_2dyz,1];[points2_line2_2dyz,1]],0); 
% line2_vec_2dyz = vm(:,end); [~,~, vm] = svd([line1_vec_2dyz'; line2_vec_2dyz']); 
% ShadowEliminationPoint_yz = vm(:,end);

% 采取叉乘
line1_vec_2dxz = GetLineFromTwoPoints(points1_line1_2dxz, points2_line1_2dxz);
line2_vec_2dxz = GetLineFromTwoPoints(points1_line2_2dxz, points2_line2_2dxz);
ShadowEliminationPoint_xz = cross(line1_vec_2dxz, line2_vec_2dxz);

line1_vec_2dxy = GetLineFromTwoPoints(points1_line1_2dxy, points2_line1_2dxy);
line2_vec_2dxy = GetLineFromTwoPoints(points1_line2_2dxy, points2_line2_2dxy);
ShadowEliminationPoint_xy = cross(line1_vec_2dxy, line2_vec_2dxy);

line1_vec_2dyz = GetLineFromTwoPoints(points1_line1_2dyz, points2_line1_2dyz);
line2_vec_2dyz = GetLineFromTwoPoints(points1_line2_2dyz, points2_line2_2dyz);
ShadowEliminationPoint_yz = cross(line1_vec_2dyz, line2_vec_2dyz);

% 影消点构造系数矩阵
CoeffMat = [GetCoeffMat(ShadowEliminationPoint_xz,ShadowEliminationPoint_xy);...
    GetCoeffMat(ShadowEliminationPoint_xz,ShadowEliminationPoint_yz);...
    GetCoeffMat(ShadowEliminationPoint_xy,ShadowEliminationPoint_yz)];

[~,~,vm] = svd(CoeffMat ,0);
w1 = vm(1,end);  % 最初是vm(1)错误，应该取最后一列向量
w4 = vm(2,end);
w5 = vm(3,end);
w6 = vm(4,end);

W = [w1 0 w4; 0 w1 w5; w4 w5 w6];
%W = W^(-1);
W = W./W(end);
% cholesky 分解出相机内参
K = chol(W)^(-1);
K = K./K(end);

%影消点方法
% K =
%    1.0e+03 *
% 
%    1.298148421816522                   0   0.567508507262171
%                    0   1.298148421816522   0.489557443259937
%                    0                   0   0.001000000000000

% 上节 3d-2d点对应方法
% K =
%    1.0e+03 *
%    1.229866653253700   0.000000004705684   0.734717308517352
%                    0   1.226720702852342   0.622741888452889
%                    0                   0   0.00100000000000



% 采取svd分解
% W =
% 
%    0.000000445053333                   0  -0.000252571552416
%                    0   0.000000445053333  -0.000217879171605
%   -0.000252571552416  -0.000217879171605   1.000000000000000

% 采取叉乘方式
% W =
% 
%    0.000000445053333                   0  -0.000252571552416
%                    0   0.000000445053333  -0.000217879171604
%   -0.000252571552416  -0.000217879171604   1.000000000000000


% 构造齐次线性方程组
function vec = GetCoeffMat(points1, points2)
    u1 = points1(1);
    v1 = points1(2);
    z1 = points1(3);
    u2 = points2(1);
    v2 = points2(2);
    z2 = points2(3);

    vec = [u1*u2 + v1*v2, z1*u2+u1*z2, z2*v1+v2*z1, z1*z2];
end


% 过两点的直线向量
function directionVector = GetLineFromTwoPoints(points1, points2)
    directionVector = [points2(2) - points1(2), points1(1) - points2(1),...
       points1(2)*points2(1) - points1(1)*points2(2)];
    directionVector = directionVector./directionVector(end);
end




